int polySides = 6;
float polyX[] = {4,4,-4,-4,-2,-2};
float polyY[] = {4,-4,-4,0,0,4};

//  The function will return YES if the point x,y is inside the polygon, or
//  NO if it is not.  If the point is exactly on the edge of the polygon,
//  then the function may return YES or NO.
//
//  Note that division by zero is avoided because the division is protected
//  by the "if" clause which surrounds it.


//===========================
bool pointInPolygon(float x, float y)
{

  int i;
  int j=polySides-1;
  bool  oddNodes = false;

  for (i=0; i<polySides; i++)
  {
    if ((polyY[i]< y && polyY[j]>=y   ||   polyY[j]< y && polyY[i]>=y)   &&  (polyX[i]<=x || polyX[j]<=x))
    {
      oddNodes^=(polyX[i]+(y-polyY[i])/(polyY[j]-polyY[i])*(polyX[j]-polyX[i])<x);
    }
    j=i;
  }

  return oddNodes;
}

//===========================
float shortestDistance (float lon, float lat)
{
	int i;
	float Ax, Bx, Cx, Ay, By, Cy, a, b, c, r;
	float tempDistance;
	float distance = 999999999;


	//Iterate through sets of adjacent boundary points

	for(i=0; i<polySides; i++)
	{
		Ax = polyX[i];
		Ay = polyY[i];
		if(i==polySides-1)
		{
			Bx = polyX[0];
			By = polyY[0];
		}
		else
		{
			Bx = polyX[i+1];
			By = polyY[i+1];
		}
		Cx = lon;
		Cy = lat;


		//r represents where the perpendicular intersection of the test point intersects the line segment formed by
		//the two boundary points. If r<0 the intersection occurs outside of bounds1. If r>1 the intersection
		//occurs outside of bounds2. If 0<r<1 the intersection occurs on the line segment.

		r = ((Cx-Ax)*(Bx-Ax)+(Cy-Ay)*(By-Ay))/pow(sqrt(pow((Bx-Ax),2)+pow((By-Ay),2)),2);

		if (0<r && r<1)
		{

			 //This tests whether the given boundary line segment is a vertical line. If it is, we swap all x and y
			 //coordinates for both boundary points and the test point. This allows us to sidestep the vertical line
			 //problem and calculate the accurate distance.

			if(Ax==Bx)
			{
				a = (Bx-Ax)/(By-Ay);
				b = -1;
				c = Ax - ((Bx-Ax)/(By-Ay))*Ay;
				Cx = lat;
				Cy = lon;
			}
			else
			{
				a = (By-Ay)/(Bx-Ax);
				b = -1;
				c = Ay - ((By-Ay)/(Bx-Ax))*Ax;
			}


			 //The absolute value function was truncating our doubles to integers, so we implemented our own here.
			 //We use the point-line distance formula.

			if(a*Cy+b*Cx+c >= 0)
			{
				tempDistance = (a*Cx+b*Cy+c)/sqrt(a*a+b*b);
			}
			else
			{
				tempDistance = -1*(a*Cx+b*Cy+c)/sqrt(a*a+b*b);
			}


			 //We only want to consider positive distances- otherwise the smallest distance will be the greatest
			 //negative number

			if(tempDistance < 0)
			{
				tempDistance *= -1;
			}

			if (distance>tempDistance)
			{
				distance=tempDistance;
			}
		}

		 //If the perpendicular intersection occurs outside of the line segment, we calculate the distance to
		 //each of the two boundary points.

		else
		{
			tempDistance = sqrt(pow((Cx-Ax),2)+pow((Cy-Ay),2));
			if (distance>tempDistance)
			{
				distance=tempDistance;
			}

			tempDistance = sqrt(pow((Cx-Bx),2)+pow((Cy-By),2));
			if (distance>tempDistance)
			{
				distance=tempDistance;
			}
		}

	}
	return distance;
}


void setup(){
  
  
}

void loop(){
  
  
  
}
